#include "single_innfos_joint.h"
#include <unistd.h>
#include <iostream>

using namespace std;

int main()
{
	single_innfos_joint s_joint("ens33", 91, 201);
	s_joint.on(false);
	
	double * pos, * vel, * cur;
	s_joint.data_feedback(&pos, &vel, &cur);
	cout << "pos: " << pos << "; vel: " << vel << "; cur: " << cur << endl; 
	s_joint.poscontrol_on();
	s_joint.to_absolute_position(M_PI);

	s_joint.to_relative_position(M_PI);
	cout << "pos: " << *pos << "; vel: " << *vel << "; cur: " << *cur << endl;
	
	s_joint.velcontrol_on();
	s_joint.rotate(1200);
	s_joint.rotate_stop();
	//cout << "pos: " << *pos << "; vel: " << *vel << "; cur: " << *cur << endl;
	s_joint.rotate_time(1200, 1000);
	//cout << "pos: " << *pos << "; vel: " << *vel << "; cur: " << *cur << endl;
	s_joint.rotate_cur_limit(-1200, 0.8);
	//cout << "pos: " << *pos << "; vel: " << *vel << "; cur: " << *cur << endl;
	
	s_joint.poscontrol_on();
	s_joint.to_absolute_position(0);
	//cout << "pos: " << *pos << "; vel: " << *vel << "; cur: " << *cur << endl;
	
	s_joint.off();
	return 0;
}
